#ifndef GROOVE_DETECTION_H
#define GROOVE_DETECTION_H

#include <algorithm>
#include <iostream>
#include <math.h>
#include <numeric>
#include <string>

#include <pcl/ModelCoefficients.h>
#include <pcl/common/common_headers.h>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/surface/convex_hull.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
class GrooveDetection {
public:
  typedef pcl::PointXYZ PointT;
  typedef pcl::PointCloud<PointT> PointCloudT;
  GrooveDetection();
  ~GrooveDetection();
  void setInputCloud(PointCloudT::Ptr &cloud_in);

  int compute();  //开始计算

  int groove_num = 0;                       //沟槽数目
  vector<float> groove_depth;               //槽深
  vector<float> groove_width;               //槽宽
  vector<float> groove_length;              //槽长
  vector<PointCloudT::Ptr> grooveCloud_vec; //沟槽点云

  Eigen::Affine3d trans2plane_matrix = Eigen::Affine3d::Identity();

private:
  PointCloudT::Ptr cloud_src;
  //计算平面到xoy面，即z=0平面的变换矩阵
  void Trans2Plane(pcl::ModelCoefficients::Ptr &plan_coeff,
                   Eigen::Affine3d &trans_matrix);
  //计算直线到y轴方向，即(0,1,0)的变换矩阵
  void Trans2line(Eigen::Vector3f &vecNormal, Eigen::Affine3f &trans_matrix);
  //计算槽深
  float compute_depth(PointCloudT::Ptr &cloud_in);
  //计算槽宽
  float compute_width(PointCloudT::Ptr &cloud_in, Eigen::Vector3f &vecNormal,
                      int cluster_id);
  //计算槽长度
  float compute_length(PointCloudT::Ptr &cloud_in);

  //将点云投影到z=0平面
  void proj2z0(PointCloudT::Ptr &cloud_in, PointCloudT::Ptr &cloud_out,
               float zlimit);
  //拟合两条边界线
  void lineFit(PointCloudT::Ptr &cloud_proj,
               pcl::ModelCoefficients::Ptr &line_coeff1,
               pcl::ModelCoefficients::Ptr &line_coeff2, int cluster_id);
  //法线约束再次拟合
  void lineFitWithNormal(PointCloudT::Ptr &cloud_proj,
                         pcl::ModelCoefficients::Ptr &line_coeff1,
                         pcl::ModelCoefficients::Ptr &line_coeff2,
                         Eigen::Vector3f &normal, int cluster_id);
};

#endif // GROOVE_DETECTION_H
